#include "armor_detect.h"
#include "rp_predict.h"
#include <vector>


PredictMove kalman1;
Distance_filter df(&kalman1);

Point get_max_x_point(vector<Point> points)
{
    Point Right = *max_element(points.begin(), points.end(),
      [](const Point& lhs, const Point& rhs) {return lhs.x < rhs.x;});
    return Right;
}

Point get_min_x_point(vector<Point> points)
{
    Point Left = *min_element(points.begin(), points.end(),
      [](const Point& lhs, const Point& rhs) {return lhs.x < rhs.x;});
    return Left;
}

Point get_max_y_point(vector<Point> points)
{
    Point Top = *max_element(points.begin(), points.end(),
      [](const Point& lhs, const Point& rhs) {return lhs.y < rhs.y;});
    return Top;
}

Point get_min_y_point(vector<Point> points)
{
    Point Bottom = *min_element(points.begin(), points.end(),
      [](const Point& lhs, const Point& rhs) {return lhs.y < rhs.y;});
    return Bottom;
}

Point get_center(vector<Point> points)
{
    Moments mu;
    mu = moments(points);
    return Point(mu.m10/mu.m00, mu.m01/mu.m00);
}


int get_cale_dis(Point pt1, Point pt2)
{
    return sqrt(pow((pt1.x - pt2.x), 2) + pow((pt1.y - pt2.y), 2));
}

void make_roi_safe(Rect &roi)
{
    if( roi.x < 0 )
        roi.x = 0;
    if( roi.y < 0)
        roi.y = 0;
    if( roi.x  > VIDEO_WIDTH)
    {
        roi.x = 0;
        roi.y = 0;
        roi.width = VIDEO_WIDTH;
        roi.height = VIDEO_HEIGHT;
    }
    if( roi.y  > VIDEO_HEIGHT)
    {
        roi.x = 0;
        roi.y = 0;
        roi.width = VIDEO_WIDTH;
        roi.height = VIDEO_HEIGHT;
    }
    if( roi.x + roi.width > VIDEO_WIDTH)
        roi.width = VIDEO_WIDTH - roi.x;
    if( roi.y + roi.height > VIDEO_HEIGHT)
        roi.height = VIDEO_HEIGHT - roi.y;
}

void name_and_show(string name,Mat& img)
{
    namedWindow(name,0);
    imshow(name,img);
}

void Data::reset()
{
    mode = 0;
    shoot_speed = 15;
    my_color = 1;
    gyro_yaw = 0;
    gyro_pitch = 0;
}

void Armor::armor_reset()
{
    pitch = 0;
    yaw = 0;
    distance = 0;
    type = 2;
    ID = 0;
    area = 0;
    left_top = Point(0,0);
    right_top = Point(0,0);
    left_bottom = Point(0,0);
    right_bottom = Point(0,0);
    armor_rect = RotatedRect();
    empty = 1;
}

void ArmorDetector::reset()
{
//    cout<<"armordetector reset"<<endl;
//    last_point.clear();
    lights.clear();
    first_result.clear();
    first_result_index.clear();
    result.clear();
    target_armor.empty = 1;
}

void ArmorDetector::ctrl_reset()
{
    reset();
    drop_frame_cnt = 25;
    pre_target.empty = 1;
    roi_init = 0;
    target_armor.armor_reset();
}

Light ArmorDetector::adjust_light(Light &light)
{
    if(light.light_rect.size.width == MIN(light.light_rect.size.width, light.light_rect.size.height))
    {
        light.light_rect.angle = light.light_rect.angle<0.0001? 0 : 180+light.light_rect.angle ;
    }
    else
    {
        light.light_rect.angle = 90+light.light_rect.angle;
        swap(light.light_rect.size.width, light.light_rect.size.height);
    }

}


void ArmorDetector::adjust_rect(RotatedRect &rect)
{
    if(rect.size.width == MIN(rect.size.width, rect.size.height))
    {
        rect.angle = - (rect.angle +90);
    }
}

void ArmorDetector::set_img(bool use_roi)
{
    if(src.empty())
    {
        cout<<"src is empty"<<endl;
        return;
    }
    src.copyTo(img);
    if(img.empty())
    {
        cout<<"img is empty"<<endl;
        return;
    }
    roi_rect = Rect(0, 0, VIDEO_WIDTH, VIDEO_HEIGHT);
    if(use_roi)
    {
        if(!roi_init)
        {
            roi_rect = Rect(0, 0, VIDEO_WIDTH, VIDEO_HEIGHT);
            roi_init = 1;
        }
        else
        {
            if(pre_target.empty)
            {
                if(drop_frame_cnt >= ROI_DROP_FRAME_BOUND)
                {
                    roi_rect = Rect(0, 0, VIDEO_WIDTH, VIDEO_HEIGHT);
                }
                else
                {
                    if(drop_frame_cnt <= 5)
                    {
                        make_roi_safe(roi_rect);
                        img(roi_rect).copyTo(img);
                    }
                    else if( drop_frame_cnt <= 15 && drop_frame_cnt > 5)
                    {
                        double roi_width_scale =  DROP_ROI_WIDTH_SCALE;
                        //y不变,x变
                        int x = roi_rect.tl().x;
                        int y = roi_rect.tl().y;
                        int w = roi_rect.width;
                        int h = roi_rect.height;
                        x = x - w * roi_width_scale;
                        w = w *(1 + 2*roi_width_scale);
                        roi_rect = Rect(x, y, w, h);
                        make_roi_safe(roi_rect);
#ifdef SHOW_SRC
                        rectangle(src, roi_rect, Scalar(255, 255, 255),2, LINE_AA, 0);
#endif
                        img(roi_rect).copyTo(img);
                    }
                    if( drop_frame_cnt <= 20 && drop_frame_cnt >15)
                    {
                        double roi_height_scale =  DROP_ROI_WIDTH_SCALE;
                        int x = 0;
                        int y = roi_rect.tl().y;
                        int w = 1280;
                        int h = roi_rect.height;
                        y = y - h * roi_height_scale;
                        h = h *(1 + 2*roi_height_scale);
                        roi_rect = Rect(x, y, w, h);
                        make_roi_safe(roi_rect);
//                        cout<<"roi: "<<x<<" "<<y<<" "<<w<<" "<<h<<endl;
#ifdef SHOW_SRC
                        rectangle(src, roi_rect, Scalar(255, 255, 255),2, LINE_AA, 0);
#endif
                        img(roi_rect).copyTo(img);
                    }


                }
            }
            else
            {
                double roi_height_scale = ROI_HEIGHT_SCALE;
                double roi_width_scale = ROI_WIDTH_SCALE;
                Rect roi = pre_target.armor_rect.boundingRect();
                int x = roi.x - roi_width_scale*roi.width;
                int y = roi.y - roi_height_scale*roi.height;
                int w = roi.width*(1+2*roi_width_scale);
                int h = roi.height*(1+2*roi_height_scale);
                roi_rect = Rect(x, y, w, h);
                make_roi_safe(roi_rect);
                rectangle(src, roi_rect, Scalar(255, 255, 255),2, LINE_AA, 0);
                img(roi_rect).copyTo(img);
            }
        }
    }
}

void ArmorDetector::pretreat_img(int my_color, int mode)
{
    cout << "my_color "<<my_color << endl;
    if(img.empty())
    {
        cout<<"Img is empty"<<endl;
        return;
    }
    vector<Mat>channels;
    split(img,channels);
    Mat single_img;

    int red_thresh = RED_THRESH;
    int blue_thresh = BLUE_THRESH;
    if(my_color == 1)
    {
        if(pre_target.distance > 4800)
            red_thresh = RED_THRESH_F;
    }
    else if(my_color == 2)
    {
        if(pre_target.distance > 6000)
            blue_thresh = BLUE_THRESH_F;
    }

    Mat gray_img;
    threshold(channels[1],gray_img,GRAY_THRESH,255,CV_THRESH_BINARY);

    if(my_color == 1)            //hit red
    {
        single_img = channels[0] - channels[2];
        threshold(single_img, binary_img, red_thresh,255,CV_THRESH_BINARY);
    }
    else if(my_color == 2)      //hit blue
    {
        single_img = channels[2] - channels[0];
        threshold(single_img, binary_img, BLUE_THRESH,255,CV_THRESH_BINARY);
    }

    bitwise_and(binary_img,gray_img,binary_img);


#ifdef SHOW_IMG
    namedWindow("binary_img",0);
    imshow("binary_img",binary_img);
#endif
}

void ArmorDetector::find_light()
{
#ifdef DRAW_LIGHT
    Mat black_graph = Mat::zeros(binary_img.size(), CV_8UC3);
    namedWindow("found",0);

#endif DRAW_LIGHT
    if(binary_img.empty())
    {
        cout<<"binary_img is empty"<<endl;
        return;
    }
    vector<vector<Point>> light_contours;
    findContours(binary_img, light_contours, CV_RETR_EXTERNAL, CV_CHAIN_APPROX_SIMPLE);
    reset();
    vector<vector<Point>> light_filter;
    for(const auto& contour : light_contours)
    {
#ifdef LIGHT_INFO
        cout<< "----------a light ------------ "<<endl;
#endif
        Light light;
        light.area = contourArea(contour);
        if(light.area < MIN_LIGHT_AREA )
        {
#ifdef LIGHT_INFO
            cout<< "area 筛除的: "<<endl;
            cout<< light.area<<endl;
#endif
#ifndef DRAW_LIGHT
            continue;
#endif
        }
        else
        {
#ifdef LIGHT_INFO
            cout<< "area 未筛除: "<<endl;
            cout<< light.area <<endl;
#endif
        }
        if(contour.size()<5)
        {
            cout<< "contour size < 5 : contour size="<<contour.size()<<endl;
            light.light_rect=minAreaRect(contour);
            adjust_light(light);
        }
        else
        {
            light.light_rect=fitEllipse(contour);
        }

        double light_aspect_ratio = light.light_rect.size.height / light.light_rect.size.width;
        bool judge1 = light_aspect_ratio < MIN_LIGHT_ASPECT_RATIO;
        bool judge2 = light_aspect_ratio > MAX_LIGHT_ASPECT_RATIO;
        double judge3;
        if (light.light_rect.angle<=90)
        {
            judge3=light.light_rect.angle>RIGHT_TILT_ANGLE;
        }
        else
        {
            judge3=light.light_rect.angle<LEFT_TILT_ANGLE;
        }


        if( judge1 || judge2 || judge3)
        {
#ifdef LIGHT_INFO
            cout<<"筛除的"<<endl;
            cout<<"  1 :"<<judge1<<"  2 :"<<judge2 <<"  3 :"<<judge3;
            cout<<"灯条长宽比: "<<light_aspect_ratio<<endl;
            cout<<"灯条倾斜角度: "<<light.light_rect.angle<<endl;
#endif
#ifndef DRAW_LIGHT
            continue;
#endif
        }
        else
        {
#ifdef LIGHT_INFO
//            cout<<"没有筛除的"<<endl;
//            cout<<"灯条长宽比: "<<light_aspect_ratio<<endl;
//            cout<<"灯条倾斜角度: "<<fabs(light.light_rect.angle)<<endl;
//            cout<<"灯条的倾斜方向" <<light.titl_direction<<endl;
#endif
        }
        light.titl_direction=light.light_rect.angle<90? 1:0;
        light.top = get_min_y_point(contour)+roi_rect.tl();
        light.bottom = get_max_y_point(contour)+roi_rect.tl();
        light.center = (light.top + light.bottom) / 2;
        light_filter.push_back(contour);
        lights.push_back(light);

#ifdef DRAW_LIGHT
/////debug
///
        cout<<" center " <<light.light_rect.center<<"  width "<<light.light_rect.size.width<<"   height "<<light.light_rect.size.height<<endl;
        cout<<" angle " <<light.light_rect.angle<<endl;
        cout<<"title dir "<<light.titl_direction <<endl;
        vector<vector<Point>> cts;
        cts.push_back(contour);
        drawContours(black_graph, cts,-1 , Scalar(0,0,255), -1);
        cout<<light.light_rect.center<<endl;
        ellipse(black_graph,light.light_rect,Scalar(255,255,255),2);
        imshow("found",black_graph);
        waitKey(0);

//////end debug
///
#endif DRAW_LIGHT
    }

#ifdef SHOW_IMG
        Mat black_graph2 = Mat::zeros(binary_img.size(), binary_img.type());
        if(light_filter.size() > 0)
        {
            drawContours(black_graph2, light_filter, -1, Scalar(255,255,255), -1);
//            for(int k=0;k<lights.size();k++)
//            {
//                cout<<lights[k].light_rect.center<<endl;
//                ellipse(black_graph,lights[k].light_rect,Scalar(255,255,255),6);
//            }
        }

        namedWindow("contours_filter", WINDOW_NORMAL);
        imshow("contours_filter", black_graph2);
        waitKey(1);
#endif
}

void ArmorDetector::match_lights(int mode)
{
    Armor_params ap(mode);
    if(lights.empty())
    {
        return;
    }
    sort(lights.begin(), lights.end(), [](Light& light1,Light& light2)
          {return light1.center.x < light2.center.x ;});

    for(int i = 1; i < lights.size(); i++)
    {
        for(int j = 0; j < i; j++)
        {
#ifdef DRAW_LIGHT_MATCH
            cout<<"======i : "<<i<<" =========j : "<<j<<endl;
            Mat black_graph = Mat::zeros(binary_img.size(), CV_8UC3);
            namedWindow("found",0);
#endif LIGHT_MATCH
            right_light = lights[i];
            left_light = lights[j];
            double armor_width = get_cale_dis(right_light.center, left_light.center);
            double armor_height = (right_light.light_rect.size.height + left_light.light_rect.size.height) / 2;
            double armor_aspect_ratio = armor_width/armor_height ;
            double armor_height_ratio = right_light.light_rect.size.height / left_light.light_rect.size.height;
            armor_height_ratio = armor_height_ratio > 1 ? ( 1 / armor_height_ratio ):armor_height_ratio;

            double right_titl_angle=right_light.light_rect.angle<90 ? right_light.light_rect.angle:180-right_light.light_rect.angle;
            double left_titl_angle=left_light.light_rect.angle<90 ? left_light.light_rect.angle:180-left_light.light_rect.angle;
            double titl_differ  = fabs(right_titl_angle-left_titl_angle);

            double armor_tilt_angle1 = fabs((double)(right_light.top.y - left_light.top.y) / (right_light.top.x - left_light.top.x));
            double armor_tilt_angle2 = fabs((double)(right_light.bottom.y - left_light.bottom.y) / (right_light.bottom.x - left_light.bottom.x));
            double armor_tilt_angle = MAX(armor_tilt_angle1, armor_tilt_angle2);

            double y_diff = fabs(right_light.center.y - left_light.center.y);
            double y_diff_ratio = y_diff / armor_height;
            vector<Point> vPolygonPoint;
            vPolygonPoint.push_back(right_light.top);
            vPolygonPoint.push_back(right_light.bottom);
            vPolygonPoint.push_back(left_light.top);
            vPolygonPoint.push_back(left_light.bottom);
            RotatedRect armor_rect = minAreaRect(vPolygonPoint);
            adjust_rect(armor_rect);

            bool judge6,judge1,judge2,judge8 ;
            bool judge3 = armor_height_ratio > ap.min_height_ratio;
            bool judge4 = fabs(armor_rect.angle) < ap.min_armor_rect_angle;
            bool judge5 ;

            if(right_light.titl_direction==left_light.titl_direction)
            {
                judge5=titl_differ<ap.angle_differ1;
                judge8 = y_diff_ratio < ap.max_y_diff_ratio1;
            }
            else
            {
                judge5=titl_differ<ap.angle_differ2;
                judge8 = y_diff_ratio < ap.max_y_diff_ratio2;
            }

            if(armor_aspect_ratio < 3.4 )
            {
                judge1 = armor_aspect_ratio > ap.small_min_armor_aspect_ratio ;
                judge2 = armor_aspect_ratio < ap.small_max_armor_aspect_ratio;
                judge6 = armor_tilt_angle < ap.max_small_armor_tilt_angle;
            }
            else
            {
                judge1 = armor_aspect_ratio > ap.big_min_armor_aspect_ratio ;
                judge2 = armor_aspect_ratio < ap.big_max_armor_aspect_ratio;
                judge6 = armor_tilt_angle < ap.max_big_armor_tilt_angle;
            }



            if(judge1 && judge2 && judge3 && judge4 && judge5 && judge6 && judge8 )
            {
                first_result.push_back(left_light);
                first_result_index.push_back(j);
                first_result.push_back(right_light);
                first_result_index.push_back(i);

#ifdef ARMOR_INFO
                cout<<"未筛除的"<<endl;
                cout<<"长宽比: "<<armor_aspect_ratio << " armor_height: "<<armor_height<<" armor_width: "<<armor_width<<endl;
                cout<<"高度比: "<<armor_height_ratio <<endl;
                cout<<"titl_differ: "<<titl_differ <<endl;
                cout<<"倾斜角度"<<armor_tilt_angle<<endl;
                cout<<"y值的偏移量： "<<y_diff_ratio<<endl;
                cout<<"旋转矩形倾斜角度： "<<armor_rect.angle<<endl;
                if(right_light.titl_direction == left_light.titl_direction)
                {
                    cout<<"同向"<<endl;
                }
                else
                {
                    cout<<"异向"<<endl;
                }
#endif

            }
            else
            {
#ifdef ARMOR_INFO
                cout<<"************************shai chu le "<<endl;
                if(judge1 == 0)cout<<"judge1"<<endl;
                if(judge2 == 0)cout<<"judge2"<<endl;
                if(judge3 == 0)cout<<"judge3"<<endl;
                if(judge4 == 0)cout<<"judge4"<<endl;
                if(judge5 == 0)cout<<"judge5"<<endl;
                if(judge6 == 0)cout<<"judge6"<<endl;
                if(judge8 == 0)cout<<"judge8"<<endl;
                cout<<"筛除的"<<endl;
                cout<<"armor_aspect_ratio: "<<armor_aspect_ratio <<endl;
                cout<<"3高度比: "<<armor_height_ratio <<"     ["<<ap.min_height_ratio<<", ]"<<endl;
                cout<<"4旋转矩形角度： "<<armor_rect.angle<<"    [ ,"<<ap.min_armor_rect_angle<<"]"<<endl;
                cout<<"5平行度: "<<titl_differ <<"    ap.angle_differ1:"<<ap.angle_differ1 <<"  ap.angle_differ2"<<ap.angle_differ2<<endl;
                cout<<"6倾斜角度"<<armor_tilt_angle<<"     small: "<<ap.max_small_armor_tilt_angle<<" big:"<<ap.max_big_armor_tilt_angle<<endl;
                cout<<"8y值的偏移量"<<y_diff_ratio<<"  ap.max_y_diff_ratio:"<<ap.max_y_diff_ratio1<<"   ap.max_y_diff_ratio:"<<ap.max_y_diff_ratio2<<endl;
#endif
            }
#ifdef DRAW_LIGHT_MATCH
            ellipse(black_graph,right_light.light_rect,Scalar(255,255,255),2);
            ellipse(black_graph,left_light.light_rect,Scalar(255,255,255),2);
            imshow("found",black_graph);
            waitKey(0);
#endif

        }
    }
#ifdef SHOW_SRC
    for(size_t i = 0; i < first_result.size();i = i+ 2)
    {
        vector<Point> vPolygonPoint;
        vPolygonPoint.push_back(first_result[i].top);
        vPolygonPoint.push_back(first_result[i].bottom);
        vPolygonPoint.push_back(first_result[i+1].top);
        vPolygonPoint.push_back(first_result[i+1].bottom);
        RotatedRect armor_rect = minAreaRect(vPolygonPoint);

        Point2f vertex[4];
        armor_rect.points(vertex);
        for (int j = 0; j < 4; j++)
        {
            line(src, vertex[j], vertex[(j + 1) % 4], Scalar(125, 0, 125), 2, CV_AA);
        }
    }

#endif

}

void ArmorDetector::match_lights_deeper()
{
    if(first_result_index.empty() || first_result.empty())
    {
        return;
    }

    vector<int> no_i;        //为了存放不是三灯条的灯条
    vector<int> no_double_i; //三灯条后舍弃不用的灯条


    for(size_t i = 0; i < first_result_index.size(); i++)
    {
#ifdef DRAW_MATCH_DEEPER
            cout<<"match light:=================== i:"<<i<<endl;
            Mat black_graph = Mat::zeros(binary_img.size(), CV_8UC3);
            namedWindow("match_deeper",0);
#endif
        int _flag = 0;
        for(int k = 0; k < no_double_i.size(); k++)
        {
            if( i == no_double_i[k])
            {
                _flag = 1;
#ifdef MATCH_DEEPER
                cout<<"跳过"<<no_double_i[k]<<endl;
#endif
                break;
            }
        }
        if(_flag == 1)
            continue;

        for(size_t j = i + 1; j < first_result_index.size(); j++)
        {
#ifdef MATCH_DEEPER
        cout<<"i_index: "<<first_result_index[i]<<"i: "<<i<<endl;
        cout<<"j_index: "<<first_result_index[j]<<"j: "<<j<<endl;
#endif
            if(first_result_index[i] == first_result_index[j])
            {
                if( i % 2 == 0 && j %2 == 0 ) //公用灯条在左边，0 1 0 2 型， 选01装甲板，因为不存在装甲板中间有灯条
                {
#ifdef MATCH_DEEPER
                    cout<<"公用灯条在左边"<<endl;
#endif
                    result.push_back(first_result[i]);
                    result.push_back(first_result[i+1]);
                    no_i.push_back(i+1);
                    no_i.push_back(j+1);
                    no_double_i.push_back(j);
                    no_double_i.push_back(j+1);

                }
                else if( i % 2 == 1 && j %2 == 1)//公用灯条在右边，0 1 2 1 型， 选21装甲板，因为不存在装甲板中间有灯条
                {
#ifdef MATCH_DEEPER
                    cout<<"公用灯条在右边"<<endl;
#endif
                    result.push_back(first_result[j-1]);
                    result.push_back(first_result[j]);
                    no_i.push_back(i);
                    no_i.push_back(j);
                    no_double_i.push_back(j);
                    no_double_i.push_back(j-1);
                }
//                else // 公用灯条在中间，0 1 1 2 型， 选同向的装甲板
//                {
//                    double left_k1 = fabs((double)(first_result[i].top.y - first_result[i-1].top.y)/(double)(first_result[i].top.x - first_result[i-1].top.x));
//                    double left_k2 = fabs((double)(first_result[i].bottom.y - first_result[i-1].bottom.y)/(double)(first_result[i].bottom.x - first_result[i-1].bottom.x));
//                    double left_k = MAX(left_k1, left_k2);
//                    double right_k1 = fabs((double)(first_result[j].top.y - first_result[j+1].top.y)/(double)(first_result[j].top.x - first_result[j+1].top.x));
//                    double right_k2 = fabs((double)(first_result[j].bottom.y - first_result[j+1].bottom.y)/(double)(first_result[j].bottom.x - first_result[j+1].bottom.x));
//                    double right_k = MAX(right_k1, right_k2);

//                    double thre_for_four = 0;
//                    if((target_armor.distance == 0) || (target_armor.distance >= 1000) )
//                    {
//                        thre_for_four = 0.1;
//                    }
//                    else
//                    {
//                        thre_for_four = 0.05;
//                    }

//                    if( left_k < right_k && (left_k > thre_for_four || right_k > thre_for_four) )
//                    {
//#ifdef MATCH_DEEPER
////                        cout<<"i"<<lights_parallelism1<<"  "<<k1<<endl;
////                        cout<<"j"<<lights_parallelism2<<"  "<<k2<<endl;
//                        cout<<"选择左边"<<endl;
//                        cout<<"left: "<<left_k<<endl;
//                        cout<<"right: "<<right_k<<endl;
//#endif
//                        result.push_back(first_result[i-1]);
//                        result.push_back(first_result[i]);
//                        no_double_i.push_back(j);
//                        no_double_i.push_back(j+1);
//                    }
//                    else if(left_k > right_k && (left_k > thre_for_four || right_k > thre_for_four))
//                    {
//#ifdef MATCH_DEEPER
////                        cout<<"i"<<lights_parallelism1<<" "<<first_result[i].light_rect.angle<<" "<<first_result[i-1].light_rect.angle<<endl;
////                        cout<<"j"<<lights_parallelism2<<" "<<first_result[j].light_rect.angle<<" "<<first_result[j+1].light_rect.angle<<endl;
//                        cout<<"选择右边"<<endl;
//                        cout<<"left: "<<left_k<<endl;
//                        cout<<"right: "<<right_k<<endl;
//#endif
//                        result.push_back(first_result[j]);
//                        result.push_back(first_result[j+1]);
//                        no_double_i.push_back(i);
//                        no_double_i.push_back(i-1);
//                    }
//                    else if(left_k <= thre_for_four && right_k <= thre_for_four)
//                    {

//#ifdef MATCH_DEEPER
//                        cout<<"装甲板斜率太小， 选长宽比小的"<<endl;
//#endif
//                        double armor_width_l = get_cale_dis(first_result[i-1].center, first_result[i].center);
//                        double armor_height_l = (first_result[i-1].light_rect.size.width + first_result[i].light_rect.size.width) / 2;
//                        double armor_aspect_ratio_l = armor_width_l / armor_height_l;
//                        double armor_width_r = get_cale_dis(first_result[j].center, first_result[j+1].center);
//                        double armor_height_r = (first_result[j].light_rect.size.width + first_result[j+1].light_rect.size.width) / 2;
//                        double armor_aspect_ratio_r = armor_width_r / armor_height_r;
//                        if(armor_aspect_ratio_l <armor_aspect_ratio_r)
//                        {
//#ifdef MATCH_DEEPER
//                            cout<<"选取左边: "<<endl;
//                            cout<<"Left: "<<left_k1<<" "<<left_k2<<" "<<"Right: "<<right_k1<<" "<<right_k2<<endl;
//                            cout<<"Left: "<<left_k<<" "<<"Right: "<<right_k<<endl;
//                            cout<<"Left: "<<armor_aspect_ratio_l<<" Right: "<<armor_aspect_ratio_r<<endl;
//#endif
//                            result.push_back(first_result[i-1]);
//                            result.push_back(first_result[i]);
//                            no_double_i.push_back(j);
//                            no_double_i.push_back(j+1);
//                        }
//                        else
//                        {
//#ifdef MATCH_DEEPER
//                            cout<<"选取右边: "<<endl;
//                            cout<<"Left: "<<armor_aspect_ratio_l<<" Right: "<<armor_aspect_ratio_r<<endl;
//#endif

//                            result.push_back(first_result[j]);
//                            result.push_back(first_result[j+1]);
//                            no_double_i.push_back(i);
//                            no_double_i.push_back(i-1);
//                        }
//                    }

//                    no_i.push_back(i);
//                    no_i.push_back(j+1);

//                }
                else    //选同向的
                {
                    int titl_same1=0;
                    int titl_same2=0;
                    if (first_result[i].titl_direction==first_result[i-1].titl_direction || abs(90-first_result[i].light_rect.angle)>ANGLE_NEAR_ZERO || abs(90-first_result[i-1].light_rect.angle)>ANGLE_NEAR_ZERO)
                    {
                        titl_same1=1;
                    }
                    if (first_result[j].titl_direction==first_result[j+1].titl_direction || abs(90-first_result[j].light_rect.angle)>ANGLE_NEAR_ZERO || abs(90-first_result[j+1].light_rect.angle)>ANGLE_NEAR_ZERO)
                    {
                        titl_same2=1;
                    }
                    if(titl_same1==1 && titl_same2==0)
                    {
#ifdef MATCH_DEEPER
                        cout<<"选择左边,因为左边同向"<<endl;
#endif
                        result.push_back(first_result[i-1]);
                        result.push_back(first_result[i]);
                        no_double_i.push_back(j);
                        no_double_i.push_back(j+1);
                    }
                    else if(titl_same1==0 && titl_same2==1)
                    {
#ifdef MATCH_DEEPER
                        cout<<"选择右边，右边同向"<<endl;
#endif
                        result.push_back(first_result[j]);
                        result.push_back(first_result[j+1]);
                        no_double_i.push_back(i);
                        no_double_i.push_back(i-1);
                    }
                    else
                    {
                        double left_k1 = fabs((double)(first_result[i].top.y - first_result[i-1].top.y)/(double)(first_result[i].top.x - first_result[i-1].top.x));
                        double left_k2 = fabs((double)(first_result[i].bottom.y - first_result[i-1].bottom.y)/(double)(first_result[i].bottom.x - first_result[i-1].bottom.x));
                        double left_k = MAX(left_k1, left_k2);
                        double right_k1 = fabs((double)(first_result[j].top.y - first_result[j+1].top.y)/(double)(first_result[j].top.x - first_result[j+1].top.x));
                        double right_k2 = fabs((double)(first_result[j].bottom.y - first_result[j+1].bottom.y)/(double)(first_result[j].bottom.x - first_result[j+1].bottom.x));
                        double right_k = MAX(right_k1, right_k2);

                        double thre_for_four = 0;
                        if((target_armor.distance == 0) || (target_armor.distance >= 1000) )
                        {
                            thre_for_four = 0.1;
                        }
                        else
                        {
                            thre_for_four = 0.05;
                        }

                        if( left_k < right_k && (left_k > thre_for_four || right_k > thre_for_four) )
                        {
    #ifdef MATCH_DEEPER
    //                        cout<<"i"<<lights_parallelism1<<"  "<<k1<<endl;
    //                        cout<<"j"<<lights_parallelism2<<"  "<<k2<<endl;
                            cout<<"选择左边"<<endl;
                            cout<<"left: "<<left_k<<endl;
                            cout<<"right: "<<right_k<<endl;
    #endif
                            result.push_back(first_result[i-1]);
                            result.push_back(first_result[i]);
                            no_double_i.push_back(j);
                            no_double_i.push_back(j+1);
                        }
                        else if(left_k > right_k && (left_k > thre_for_four || right_k > thre_for_four))
                        {
    #ifdef MATCH_DEEPER
    //                        cout<<"i"<<lights_parallelism1<<" "<<first_result[i].light_rect.angle<<" "<<first_result[i-1].light_rect.angle<<endl;
    //                        cout<<"j"<<lights_parallelism2<<" "<<first_result[j].light_rect.angle<<" "<<first_result[j+1].light_rect.angle<<endl;
                            cout<<"选择右边"<<endl;
                            cout<<"left: "<<left_k<<endl;
                            cout<<"right: "<<right_k<<endl;
    #endif
                            result.push_back(first_result[j]);
                            result.push_back(first_result[j+1]);
                            no_double_i.push_back(i);
                            no_double_i.push_back(i-1);
                        }
                        else if(left_k <= thre_for_four && right_k <= thre_for_four)
                        {

    #ifdef MATCH_DEEPER
                            cout<<"装甲板斜率太小， 选长宽比小的"<<endl;
    #endif
                            double armor_width_l = get_cale_dis(first_result[i-1].center, first_result[i].center);
                            double armor_height_l = (first_result[i-1].light_rect.size.width + first_result[i].light_rect.size.width) / 2;
                            double armor_aspect_ratio_l = armor_width_l / armor_height_l;
                            double armor_width_r = get_cale_dis(first_result[j].center, first_result[j+1].center);
                            double armor_height_r = (first_result[j].light_rect.size.width + first_result[j+1].light_rect.size.width) / 2;
                            double armor_aspect_ratio_r = armor_width_r / armor_height_r;
                            if(armor_aspect_ratio_l <armor_aspect_ratio_r)
                            {
    #ifdef MATCH_DEEPER
                                cout<<"选取左边: "<<endl;
                                cout<<"Left: "<<left_k1<<" "<<left_k2<<" "<<"Right: "<<right_k1<<" "<<right_k2<<endl;
                                cout<<"Left: "<<left_k<<" "<<"Right: "<<right_k<<endl;
                                cout<<"Left: "<<armor_aspect_ratio_l<<" Right: "<<armor_aspect_ratio_r<<endl;
    #endif
                                result.push_back(first_result[i-1]);
                                result.push_back(first_result[i]);
                                no_double_i.push_back(j);
                                no_double_i.push_back(j+1);
                            }
                            else
                            {
    #ifdef MATCH_DEEPER
                                cout<<"选取右边: "<<endl;
                                cout<<"Left: "<<armor_aspect_ratio_l<<" Right: "<<armor_aspect_ratio_r<<endl;
    #endif

                                result.push_back(first_result[j]);
                                result.push_back(first_result[j+1]);
                                no_double_i.push_back(i);
                                no_double_i.push_back(i-1);
                            }
                        }

                    }
                    no_i.push_back(i);
                    no_i.push_back(j+1);

                }

            }
        }

        int flag = 0;
        if(i % 2 == 1 )
        {
            for(size_t j = 0; j < no_i.size(); j++)
            {
                if(i == no_i[j])
                    flag = 1;
            }
            if(!flag)
            {
                result.push_back(first_result[i-1]);
                result.push_back(first_result[i]);
            }


        }

    }

#ifdef SHOW_SRC
    for(size_t i = 0; i < result.size();i = i+ 2)
    {
        vector<Point> vPolygonPoint;
        vPolygonPoint.push_back(result[i].top);
        vPolygonPoint.push_back(result[i].bottom);
        vPolygonPoint.push_back(result[i+1].top);
        vPolygonPoint.push_back(result[i+1].bottom);
        RotatedRect armor_rect = minAreaRect(vPolygonPoint);
        Point2f vertex[4];
        armor_rect.points(vertex);
        for (int j = 0; j < 4; j++)
        {
            line(src, vertex[j], vertex[(j + 1) % 4], Scalar(0, 255, 255), 2, CV_AA);
        }
    }
#endif
}

void ArmorDetector::choose_target(int my_color)
{
    if(result.empty())
    {
        return;
    }
    vector<Armor>armors;
    int max_index = 0;
    int min_index = 0;
    int delta_min = 100000;
    int iou_max = 0;
    //如果之前没有目标,选取面积最大
    if(pre_target.empty && result.size()> 0)
    {
        for(size_t i = 0; i < result.size(); i = i +2)
        {
            Armor armor;
            vector<Point> vPolygonPoint;
            vPolygonPoint.push_back(result[i].top);
            vPolygonPoint.push_back(result[i].bottom);
            vPolygonPoint.push_back(result[i+1].bottom);
            vPolygonPoint.push_back(result[i+1].top);
            armor.bounding_rect=boundingRect(vPolygonPoint);
            armor.armor_rect = minAreaRect(vPolygonPoint);
            armor.area = armor.armor_rect.size.width * armor.armor_rect.size.height;
            armor.left_top = result[i].top;
            armor.left_bottom = result[i].bottom;
            armor.right_bottom = result[i+1].bottom;
            armor.right_top = result[i+1].top;
            armors.push_back(armor);
        }
        for(size_t i = 1; i < armors.size(); i++)
        {
            if(armors[i].area > armors[max_index].area)
            {
                max_index = i;
            }
        }
        target_armor.left_top = armors[max_index].left_top;
        target_armor.left_bottom = armors[max_index].left_bottom;
        target_armor.right_bottom = armors[max_index].right_bottom;
        target_armor.right_top = armors[max_index].right_top;
        target_armor.armor_rect= armors[max_index].armor_rect;
        target_armor.bounding_rect= armors[max_index].bounding_rect;
        target_armor.empty = 0;
        last_point.clear();
        last_point.push_back(target_armor.left_top);
        last_point.push_back(target_armor.left_bottom);
        last_point.push_back(target_armor.right_top);
        last_point.push_back(target_armor.right_bottom);
        target_armor.type = clf.Infan_SizePredict(last_point);
    }

    //如果之前有目标，选取和上一次目标最近的装甲板，衡量标准，deltax * deltay
    else if(!pre_target.empty && result.size()> 0)
    {
        for(size_t i = 0; i < result.size(); i = i +2)
        {
            Armor armor;
            vector<Point> vPolygonPoint;
            vPolygonPoint.push_back(result[i].top);
            vPolygonPoint.push_back(result[i].bottom);
            vPolygonPoint.push_back(result[i+1].top);
            vPolygonPoint.push_back(result[i+1].bottom);
            armor.bounding_rect=boundingRect(vPolygonPoint);
            armor.armor_rect = minAreaRect(vPolygonPoint);
            armor.left_top = result[i].top;
            armor.left_bottom = result[i].bottom;
            armor.right_bottom = result[i+1].bottom;
            armor.right_top = result[i+1].top;
            armors.push_back(armor);
        }
//        for(size_t i = 0; i < armors.size(); i++)
//        {
//            double delta_x = fabs(armors[i].armor_rect.center.x - pre_target.armor_rect.center.x) ;
//            double delta_y = fabs(armors[i].armor_rect.center.y - pre_target.armor_rect.center.y) ;
//            double delta = delta_x*delta_x + delta_y*delta_y;

//            if( delta < delta_min)
//            {
//                delta_min = delta;
//                min_index = i;
//            }
//        }
        //IOU
        for(size_t i = 0; i < armors.size(); i++)
        {
            Rect bing=armors[i].bounding_rect | pre_target.bounding_rect;
            Rect jiao=armors[i].bounding_rect & pre_target.bounding_rect;
            double iou = 1.0*jiao.area()/bing.area();

            if( iou > iou_max)
            {
                iou_max = iou;
                max_index = i;
            }
        }

        target_armor.left_top = armors[max_index].left_top;
        target_armor.left_bottom = armors[max_index].left_bottom;
        target_armor.right_bottom = armors[max_index].right_bottom;
        target_armor.right_top = armors[max_index].right_top;
        target_armor.armor_rect = armors[max_index].armor_rect;
        target_armor.bounding_rect= armors[max_index].bounding_rect;
        target_armor.empty = 0;
        last_point.clear();
        last_point.push_back(target_armor.left_top);
        last_point.push_back(target_armor.left_bottom);
        last_point.push_back(target_armor.right_top);
        last_point.push_back(target_armor.right_bottom);
        target_armor.type = clf.Infan_SizePredict(last_point);
    }



#ifdef SHOW_SRC
    if(!target_armor.empty)
    {
        vector<Point> vPolygonPoint;
        vPolygonPoint.push_back(target_armor.left_top);
        vPolygonPoint.push_back(target_armor.left_bottom);
        vPolygonPoint.push_back(target_armor.right_bottom);
        vPolygonPoint.push_back(target_armor.right_top);
        circle(src, target_armor.left_top, 4, Scalar(0,0,255), -1);
        putText(src, "tl", target_armor.left_top, FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255));
        circle(src, target_armor.left_bottom, 4, Scalar(0,0,255), -1);
        putText(src, "bl", target_armor.left_bottom, FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255));
        circle(src, target_armor.right_top, 4, Scalar(0,0,255), -1);
        putText(src, "tr", target_armor.right_top, FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255));
        circle(src, target_armor.right_bottom, 4, Scalar(0,0,255), -1);
        putText(src, "rl", target_armor.right_bottom, FONT_HERSHEY_SIMPLEX, 1, Scalar(0, 0, 255));



        RotatedRect armor_rect = minAreaRect(vPolygonPoint);
        Point2f vertex[4];
        armor_rect.points(vertex);
        for (int j = 0; j < 4; j++)
        {
            line(src, vertex[j], vertex[(j + 1) % 4], Scalar(0, 0, 255), 2, CV_AA);
        }
    }

//    Rect roi = target_armor.armor_rect.boundingRect();
//    rectangle(src, roi, Scalar(255, 255, 255),2, LINE_AA, 0);

#endif
}

Mat  ArmorDetector::get_num_img(Mat ori_img)
{
    if(!target_armor.empty )
    {
        int img_size=28;

        Mat Matrix;
        Point2f sourcePoints[4], objectPoints[4];

        make_roi_safe(roi_rect);
        Mat num_img;

        sourcePoints[0]=target_armor.left_top;
        sourcePoints[1]=target_armor.right_top;
        sourcePoints[2]=target_armor.left_bottom;
        sourcePoints[3]=target_armor.right_bottom;
        for(int i=0;i<4;i++)
        {
            circle(src,sourcePoints[i],2,Scalar(255,255,255),2);
        }

        objectPoints[0].x = 0; objectPoints[0].y = 0;
        objectPoints[1].x = img_size; objectPoints[1].y = 0;
        objectPoints[2].x = 0; objectPoints[2].y = img_size;
        objectPoints[3].x = img_size; objectPoints[3].y = img_size;

        Matrix = getPerspectiveTransform(sourcePoints, objectPoints);
        warpPerspective(ori_img, num_img, Matrix, Size(img_size, img_size), INTER_LINEAR);

        return num_img;

    }

}

void ArmorDetector::get_armor_info(Mat cam,  Mat dis)
{
    if(!target_armor.empty)
    {
        vector<Point3f> obj;
        if(target_armor.type == 0)
        {
            obj=vector<Point3f>
            {
                    Point3f(-small_armor_weight, -small_armor_height, 0),	//tl
                    Point3f(small_armor_weight, -small_armor_height, 0),	//tr
                    Point3f(small_armor_weight, small_armor_height, 0),	//br
                    Point3f(-small_armor_weight, small_armor_height, 0)	//bl
            };
        }
        else if(target_armor.type == 1)
        {
            obj=vector<Point3f>
            {
                    Point3f(-big_armor_weight, -big_armor_height, 0),	//tl
                    Point3f(big_armor_weight, -big_armor_height, 0),	//tr
                    Point3f(big_armor_weight, big_armor_height, 0),	//br
                    Point3f(-big_armor_weight, big_armor_height, 0)	//bl
            };
        }

        vector<Point2f> pnts=vector<Point2f>
        {
        Point2f(target_armor.left_top),	    //tl
        Point2f(target_armor.right_top),	    //tr
        Point2f(target_armor.right_bottom),        //br
        Point2f(target_armor.left_bottom)          //bl
        };
        Mat rVec = Mat::zeros(3, 1, CV_64FC1); //init rvec
        Mat tVec = Mat::zeros(3, 1, CV_64FC1); //init tvec
        solvePnP(obj,pnts,cam,dis,rVec,tVec,false, SOLVEPNP_ITERATIVE);
        pnp_dis = sqrt(tVec.at<double>(0)*tVec.at<double>(0)+tVec.at<double>(1)*tVec.at<double>(1)+
                        tVec.at<double>(2)*tVec.at<double>(2));
//        target_armor.yaw = atan(tVec.at<double>(0)/tVec.at<double>(2)) *180 /3.14;
//        target_armor.pitch = -atan(tVec.at<double>(1)/tVec.at<double>(2)) *180 / 3.14;
        double xn=(target_armor.armor_rect.center.x-center_x)/1680;
        double yn=(target_armor.armor_rect.center.y-center_y)/1690;
        int flagy=yn>0 ? 1:-1;
        target_armor.yaw = atan(xn) *180 /3.14;
        target_armor.pitch = flagy*atan(sqrt(yn*yn/(1+xn*xn))) *180 / 3.14;
        target_armor.distance = pnp_dis;

        pre_target.distance = target_armor.distance;
        pre_target.pitch = target_armor.pitch;
        pre_target.yaw = target_armor.yaw;
        pre_target.armor_rect = target_armor.armor_rect;
        pre_target.bounding_rect=target_armor.bounding_rect;
        pre_target.empty = 0;
        drop_frame_cnt = 0;
    }
    else
    {
        pre_target.distance = 0;
        pre_target.pitch = 0;
        pre_target.yaw = 0;
        pre_target.armor_rect=RotatedRect();
        pre_target.empty = 1;
        drop_frame_cnt++;
    }

}

void ArmorDetector::get_tvec(Armor &armor, Mat cam, Mat dis)
{
    vector<Point3f> obj;
    if(armor.type == 0)
    {
        obj=vector<Point3f>
        {
                Point3f(-small_armor_weight, -small_armor_height, 0),	//tl
                Point3f(small_armor_weight, -small_armor_height, 0),	//tr
                Point3f(small_armor_weight, small_armor_height, 0),	//br
                Point3f(-small_armor_weight, small_armor_height, 0)	//bl
        };
    }
    else if(armor.type == 1)
    {
        obj=vector<Point3f>
        {
                Point3f(-big_armor_weight, -big_armor_height, 0),	//tl
                Point3f(big_armor_weight, -big_armor_height, 0),	//tr
                Point3f(big_armor_weight, big_armor_height, 0),	//br
                Point3f(-big_armor_weight, big_armor_height, 0)	//bl
        };
    }


    vector<Point2f> pnts=vector<Point2f>
    {
    Point2f(armor.left_top),	    //tl
    Point2f(armor.right_top),	    //tr
    Point2f(armor.right_bottom),	//br
    Point2f(armor.left_bottom)      //bl
    };
    Mat rVec = Mat::zeros(3, 1, CV_64FC1); //init rvec
    Mat tVec = Mat::zeros(3, 1, CV_64FC1); //init tvec

    solvePnP(obj,pnts,cam,dis,rVec,tVec,false, SOLVEPNP_ITERATIVE);
    armor.distance = sqrt(tVec.at<double>(0)*tVec.at<double>(0)+tVec.at<double>(1)*tVec.at<double>(1)+
                    tVec.at<double>(2)*tVec.at<double>(2));

    armor.yaw = atan(tVec.at<double>(0)/tVec.at<double>(2)) *180 /3.14;
    armor.pitch = -atan(tVec.at<double>(1)/tVec.at<double>(2)) *180 / 3.14;
//    armor.pitch = -atan(tVec.at<double>(1)/sqrt(tVec.at<double>(0)*tVec.at<double>(0) + tVec.at<double>(2)*tVec.at<double>(2))) * 180 /3.14;
}

void ArmorDetector::info_src(Data rdata)
{
#ifdef SHOW_SRC
    //设置绘制文本的相关参数
    string text1 = "gyro_yaw: "+ to_string(rdata.gyro_yaw);
    string text2 = "gyro_pit: " + to_string(rdata.gyro_pitch);
    string text3 = "vis_yaw: " + to_string(target_armor.yaw);
    string text4 = "vis_pit: " + to_string(target_armor.pitch);
    string text5 = "vis_dis: " + to_string(target_armor.distance);
    string text6 = "type: " + to_string(target_armor.type);


//    int drop_flag = 0;
//    if(drop_frame_cnt <= 20 && drop_frame_cnt > 0)
//    {
//        drop_flag = 1;
//    }
//    string text7 = "drop_flag: " + to_string(drop_flag);
//    string text8 = "drop_frame_cnt: " + to_string(drop_frame_cnt);
//    string text6 = "abs_yaw: " + to_string(abs_yaw);
//    string text7 = "abs_pit: " + to_string(abs_pit);
//    string text8 = "object_speed: " + to_string(info.now_frame.object_speed);
    int font_face = FONT_HERSHEY_COMPLEX;
    double font_scale = 1;
    int thickness = 2;
    int baseline;

    //获取文本框的长宽
    Size text1_size = getTextSize(text1, font_face, font_scale, thickness, &baseline);
    //将文本框居中绘制
    Point origin;
    origin.x = 20;
    origin.y = text1_size.height*1;
    putText(src, text1, origin, font_face, font_scale, Scalar(255, 0, 0), thickness, 8, 0);
    origin.y = text1_size.height * 3;
    putText(src, text2, origin, font_face, font_scale, Scalar(255, 0, 0), thickness, 8, 0);
    origin.y = text1_size.height * 5;
    putText(src, text3, origin, font_face, font_scale, Scalar(255, 0, 0), thickness, 8, 0);
    origin.y = text1_size.height * 7;
    putText(src, text4, origin, font_face, font_scale, Scalar(255, 0, 0), thickness, 8, 0);
    origin.y = text1_size.height * 9;
    putText(src, text5, origin, font_face, font_scale, Scalar(255, 0, 0), thickness, 8, 0);
    origin.y = text1_size.height * 11;
    putText(src, text6, origin, font_face, font_scale, Scalar(255, 0, 0), thickness, 8, 0);
//    origin.y = text1_size.height * 13;
//    putText(src, text7, origin, font_face, font_scale, Scalar(255, 0, 0), thickness, 8, 0);
//    origin.y = text1_size.height * 15;
//    putText(src, text8, origin, font_face, font_scale, Scalar(255, 0, 0), thickness, 8, 0);

    namedWindow("src",0);
    imshow("src",src);
    waitKey(1);
#endif
}

void ArmorDetector::armor_ctrl(bool use_roi, Mat cam, Mat dis, Data rdata)
{
    set_img(use_roi);
    pretreat_img(rdata.my_color, rdata.mode);
    find_light();
    match_lights(rdata.mode);
    match_lights_deeper();
    choose_target(rdata.my_color);
    get_armor_info(cam, dis);

}

void ArmorDetector::set_offset(double target_dis, double &target_pit, double &target_yaw, int shoot_speed, int mode, int my_color)
{
    double pit_offset = 0;
    double yaw_offset = 0;
    if(mode == 1)
    {
        if(my_color == 2) // Hit blue
        {
            if(shoot_speed == 15)
            {
                if(target_dis > 1500 )
                    yaw_offset = 0.2;
                if(target_dis <= 800)
                    pit_offset = 2.8;
                else if(target_dis > 800 && target_dis <= 1000)
                    pit_offset = 3.2;
                else if(target_dis > 1000 && target_dis <= 2000)
                    pit_offset = 1.6;
                else if(target_dis > 2000 && target_dis <= 2400)
                    pit_offset = 1.4;
                else if(target_dis > 2400 && target_dis <= 3400)
                    pit_offset = 1.8;
                else if(target_dis > 3400 && target_dis <= 4200)
                    pit_offset = 2.3;
                else if(target_dis > 4200 && target_dis <= 4700)
                    pit_offset = 3.2;
                else if(target_dis > 4700 && target_dis <= 5400)
                    pit_offset = 3.8;
                else if(target_dis > 5400 && target_dis <= 6000)
                    pit_offset = 4.65;
                else if(target_dis > 6000 && target_dis <= 6700)
                    pit_offset = 5.2;
                else if(target_dis > 6700 && target_dis <= 7500)
                    pit_offset = 6.8;
                else if(target_dis > 7500)
                    pit_offset = 6.2;
                pit_offset += 1.6;
            }
            else if(shoot_speed == 18)
            {
                if(target_dis > 3500)
                    yaw_offset = 0.2;
                if(target_dis <= 1400)
                    pit_offset = 1.3;
                else if(target_dis > 1400 && target_dis <= 2000)
                    pit_offset = 0.4;
                else if(target_dis > 2000 && target_dis <= 3200)
                    pit_offset = 0.3;
                else if(target_dis > 3200 && target_dis <= 4200)
                    pit_offset = 0.5;
                else if(target_dis > 4200 && target_dis <= 5200)
                    pit_offset = 1.1;
                else if(target_dis > 5200 && target_dis <= 6000)
                    pit_offset = 1.5;
                else if(target_dis > 6000 && target_dis <= 6700)
                    pit_offset = 1.95;
                else if(target_dis >= 6700 && target_dis <= 7000)
                    pit_offset = 2.8;
                else if(target_dis > 7000 && target_dis <= 8000)
                    pit_offset = 3.5;
                else if(target_dis > 8000)
                    pit_offset = 4.0;

                pit_offset += 2.2;
            }
            else if(shoot_speed == 30)
            {
                if(target_dis <= 1200)
                    pit_offset = -0.2;
                else if(target_dis > 1200 && target_dis <= 2000)
                    pit_offset = -2.2;
                else if(target_dis > 2000 && target_dis <= 3400)
                    pit_offset = -2.8;
                else if(target_dis > 3400 && target_dis <= 4200)
                    pit_offset = -3.15;
//                else if(target_dis > 4200 && target_dis <= 4800)
//                    pit_offset = -3.15;
//                else if(target_dis > 4800 && target_dis <= 5200)
//                    pit_offset = -2.85;
                else if(target_dis > 4200 && target_dis <= 5200)
                    pit_offset = -2.95;//sentry
                else if(target_dis > 5200 && target_dis <= 6000)
                    pit_offset = -2.75;
                else if(target_dis > 6000 && target_dis <= 6700)
                    pit_offset = -2.75;
                else if(target_dis > 6700 && target_dis <= 7400)
                    pit_offset = -2.75;
                else if(target_dis > 7400 && target_dis <= 9000)
                    pit_offset = -2.65;
                else if(target_dis > 9000)
                    pit_offset = -2.5;
                pit_offset += 4.0;
            }
        }
        else if(my_color == 1) // Hit red
        {
            if(shoot_speed == 15)
            {
                if(target_dis > 1500 )
                    yaw_offset = 0.2;
                if(target_dis <= 800)
                    pit_offset = 2.5;
                else if(target_dis > 800 && target_dis <= 1000)
                    pit_offset = 2.2;
                else if(target_dis > 1000 && target_dis <= 2200)
                    pit_offset = 2.2;
                else if(target_dis > 2200 && target_dis <= 3000)
                    pit_offset = 2.95;
                else if(target_dis > 3000 && target_dis <= 3400)
                    pit_offset = 3.8;
                else if(target_dis > 3400 && target_dis <= 3800)
                    pit_offset = 4.6 ;
                else if(target_dis > 3800 && target_dis <= 4200)
                    pit_offset = 5.75;
                else if(target_dis > 4200 && target_dis <= 5400)
                    pit_offset = 6.5;
                else if(target_dis > 5400 && target_dis <= 6200)
                    pit_offset = 6.5;
                else if(target_dis > 6200 && target_dis <= 6800)
                    pit_offset = 7.0;
                else if(target_dis > 6800 && target_dis <= 7500)
                    pit_offset = 7.4;
                else if(target_dis > 7500)
                    pit_offset = 7.4;
                pit_offset += 1;
            }
            else if(shoot_speed == 18)
            {
                if(target_dis > 3000)
                    yaw_offset = 0.2;
                if(target_dis <= 1400)
                    pit_offset = 0.6;
                else if(target_dis > 1400 && target_dis <= 2100)
                    pit_offset = 0.2;
                else if(target_dis > 2100 && target_dis <= 3200)
                    pit_offset = 0.4;
                else if(target_dis > 3200 && target_dis <= 3800)
                    pit_offset = 1.15;
                else if(target_dis > 3800 && target_dis <= 4200)
                    pit_offset = 1.8;
                else if(target_dis > 4200 && target_dis <= 4700)
                    pit_offset = 2.5;
                else if(target_dis > 4700 && target_dis <= 6000)
                    pit_offset = 3.0;
                else if(target_dis > 6000)
                    pit_offset = 2.9;
                pit_offset += 2.5;
            }
            else if(shoot_speed == 30)
            {
                if(target_dis > 3500)
                    yaw_offset = 0.2;
                if(target_dis <= 1200)
                    pit_offset = -0.8;
                else if(target_dis > 1200 && target_dis <= 2000)
                    pit_offset = -1.9;
                else if(target_dis > 2000 && target_dis <= 3000)
                    pit_offset = -2.3;
                else if(target_dis > 3000 && target_dis <= 3400)
                    pit_offset = -2.4; //sentry 3000 - 4000
                else if(target_dis > 3400 && target_dis <= 4600)
                    pit_offset = -2.2;
                else if(target_dis > 4600 && target_dis <= 5000)
                    pit_offset = -1.6;
                else if(target_dis > 5000 && target_dis <= 6000)
                    pit_offset = -1.5;
                else if(target_dis > 6000 && target_dis <= 6700)
                    pit_offset = -1.8;
                else if(target_dis > 6700 && target_dis <= 7400)
                    pit_offset = -1.2;
                else if(target_dis > 7400)
                    pit_offset = -1.2;
                pit_offset += 3.25;
            }
        }
    }
    else if(mode == 4)
    {
        if(my_color == 1)
        {
            if(shoot_speed == 15)
            {
                if(target_dis < 3400)
                    yaw_offset = -0.5;
                if(target_dis <= 1200)
                    pit_offset = 0;
                else if(target_dis > 1200 && target_dis <= 1800)
                    pit_offset = 2.8;
                else if(target_dis > 1800 && target_dis <= 2200)
                    pit_offset = 3.3;
                else if(target_dis > 2000 && target_dis <= 3400)
                    pit_offset = 5.2;
                else if(target_dis > 3400 && target_dis <= 4200)
                    pit_offset = 5.2;
                else if(target_dis > 4200 && target_dis <= 4800)
                    pit_offset = 7.5;
                else if(target_dis > 4800 && target_dis <= 5500)
                    pit_offset = 9;
                else if(target_dis > 5500 && target_dis <= 7000)
                    pit_offset = 5.6;
            }
            else if(shoot_speed == 18)
            {
                if(target_dis <= 1200)
                    pit_offset = 0;
                else if(target_dis > 1200 && target_dis <= 1800)
                    pit_offset = 2.92;
                else if(target_dis > 1800 && target_dis <= 2200)
                    pit_offset = 3.2;
                else if(target_dis > 2200 && target_dis <= 3500)
                    pit_offset = 3.35;
                else if(target_dis > 3500 && target_dis <= 4000)
                    pit_offset = 4.25;
                else if(target_dis > 4000 && target_dis <= 5500)
                    pit_offset = 4.5;
                else if(target_dis > 5500 && target_dis <= 7000)
                    pit_offset = 4.5;
                pit_offset -= 3.5;
            }
            else if(shoot_speed == 30)
            {
                if(target_dis <= 1200)
                    pit_offset = 0;
                else if(target_dis > 1200 && target_dis <= 1800)
                    pit_offset = 2;
                else if(target_dis > 1800 && target_dis <= 2200)
                    pit_offset = 1.0;
                else if(target_dis > 2200 && target_dis <= 3000)
                    pit_offset = 1.0;
                else if(target_dis > 3000 && target_dis <= 4000)
                    pit_offset = 1.0;
                else if(target_dis > 4000 && target_dis <= 5500)
                    pit_offset = 1.0;
                else if(target_dis > 5500 && target_dis <= 7000)
                    pit_offset = 1.8;
            }
        }
        else if(my_color == 2)
        {
            if(shoot_speed == 15)
            {
                if(target_dis < 3400)
                    yaw_offset = -0.5;
                if(target_dis <= 1200)
                    pit_offset = 0;
                else if(target_dis > 1200 && target_dis <= 1800)
                    pit_offset = 2.8;
                else if(target_dis > 1800 && target_dis <= 2200)
                    pit_offset = 3.3;
                else if(target_dis > 2000 && target_dis <= 3400)
                    pit_offset = 5.2;
                else if(target_dis > 3400 && target_dis <= 4200)
                    pit_offset = 5.2;
                else if(target_dis > 4200 && target_dis <= 4800)
                    pit_offset = 7.5;
                else if(target_dis > 4800 && target_dis <= 5500)
                    pit_offset = 9;
                else if(target_dis > 5500 && target_dis <= 7000)
                    pit_offset = 5.6;
            }
            else if(shoot_speed == 18)
            {
                if(target_dis <= 1200)
                    pit_offset = 0;
                else if(target_dis > 1200 && target_dis <= 1800)
                    pit_offset = 2.92;
                else if(target_dis > 1800 && target_dis <= 2200)
                    pit_offset = 3.2;
                else if(target_dis > 2200 && target_dis <= 3500)
                    pit_offset = 3.35;
                else if(target_dis > 3500 && target_dis <= 4000)
                    pit_offset = 4.25;
                else if(target_dis > 4000 && target_dis <= 5500)
                    pit_offset = 4.5;
                else if(target_dis > 5500 && target_dis <= 7000)
                    pit_offset = 4.5;
                pit_offset -= 3.5;
            }
            else if(shoot_speed == 30)
            {
                yaw_offset = -0.4;
                if(target_dis <= 1200)
                    pit_offset = 0;
                else if(target_dis > 1200 && target_dis <= 1800)
                    pit_offset = 2;
                else if(target_dis > 1800 && target_dis <= 2200)
                    pit_offset = 0.8;
                else if(target_dis > 2200 && target_dis <= 3000)
                    pit_offset = 0.8;
                else if(target_dis > 3000 && target_dis <= 4000)
                    pit_offset = 0.8;
                else if(target_dis > 4000 && target_dis <= 5500)
                    pit_offset = 0.8;
                else if(target_dis > 5800 && target_dis <= 8000)
                    pit_offset = 1.2;
            }
        }

    }
//    cout<<"距离： "<<target_dis<<" 偏移： "<<pit_offset<<" yaw "<<yaw_offset<< " 射速： "<<shoot_speed<< " my_color: "<<my_color<<endl;
//    cout<<"补偿前："<<target_pit<<endl;
    target_pit += pit_offset;
    target_yaw += yaw_offset;
//    cout<<"补偿后："<<target_pit<<endl;
}

